#include "stdafx.h"
#include "cvxBoard.h"
#include "cvxSf.h"
#include "cvxMat.h"
#include <time.h>


/************************************************************************/
/*  CvxBoard                                                            */
/************************************************************************/

CvxBoard::CvxBoard()
{

}

CvxBoard::~CvxBoard()
{

}

bool CvxBoard::SetConfigure(const CvSize &size, const float unitLength)
{
	return false;
}
// bool CvxBoard::FindCorners(const IplImage *grayImage, bool isTracking, const vector<CvxMarker>& preMarkers, vector<CvxMarker> &curMarker,
// 						 vector<Point2f> &pts2d, vector<Point3f> &pts3d)const
// {
// 	return false;
// }

bool CvxBoard::FindCorners(const IplImage *image, bool isTracking, const vector<CvxMarker>& preMarkers, 
				 vector<CvxMarker> &curMarker, vector<Point2f> &pts2d)const
{
	return false;

}

bool CvxBoard::GetInsidePoints(const IplImage *image, vector<Point2f> &extremeCorners, 
							   vector<Point2f> &pts2d)const
{
	return false;
}

void CvxBoard::DrawCorners(const IplImage *image, const vector<Point2f> &pts)const
{

}

float CvxBoard::GetUnitLength(void)const
{	
	return 0.0;
}
CvSize CvxBoard::GetPatternSize(void)const
{
	return cvSize(0, 0);
}
void CvxBoard::Get3Dpts(vector<Point3f> &pts3d) const
{

}



/************************************************************************/
/*  CvxChessboard                                                     */
/************************************************************************/

CvxChessboard::CvxChessboard()
{
	m_size = cvSize(0, 0);
	m_squareLen = 0;
}
CvxChessboard::~CvxChessboard()
{

}

bool CvxChessboard::SetConfigure(const CvSize &size, const float unitLength)
{
	m_size = size;
	m_squareLen = unitLength;

	for (int y =0; y<size.height; ++y)
	{
		for (int x = 0; x<size.width; ++x)
		{
			Point3f p;
			p.x = m_squareLen * x;
			p.y = m_squareLen * y;
			p.z = 0.0;
			m_pts3d.push_back(p);
			m_pts2d.push_back(Point2f(p.x, p.y));
		}
	}
	return true;
}

bool CvxChessboard::FindCorners(const IplImage *image, bool isTracking, const vector<CvxMarker>& preMarkers, 
						   vector<CvxMarker> &curMarker, vector<Point2f> &pts2d)const
{
	assert(image);
	pts2d.clear();
	bool isOk = CvxSF::GetChessboard(const_cast<IplImage*>(image), false, m_size, pts2d);
	if (!isOk)
	{
		return false;
	}
	assert(pts2d.size() == m_size.width * m_size.height);
	return true;
}

bool CvxChessboard::GetInsidePoints(const IplImage *image, vector<Point2f> &extremeCorners, 
									  vector<Point2f> &pts2d)const
{
	assert(image);

	//get a grayimage
	IplImage *grayImage = NULL;
	if (image->nChannels == 1)
	{
		grayImage = const_cast<IplImage *>(image);
	}
	else if (image->nChannels == 3)
	{
		grayImage = cvCreateImage(cvGetSize(image), IPL_DEPTH_8U, 1);
		assert(grayImage);
		cvCvtColor(image, grayImage, CV_BGR2GRAY);
	}

	bool ret = CvxSF::GetInsidePoints(grayImage, extremeCorners, m_size, pts2d, true);	

	if (grayImage != const_cast<IplImage *>(image))
	{
		cvReleaseImage(&grayImage);
	}
	return ret;
}

void CvxChessboard::DrawCorners(const IplImage *image, const vector<Point2f> &pts)const
{
	assert(image);
	cv::drawChessboardCorners(Mat(image), m_size, pts, true);
}

float CvxChessboard::GetUnitLength(void)const
{
	return m_squareLen;
}

CvSize CvxChessboard::GetPatternSize(void)const
{
	return m_size;
}

void CvxChessboard::Get3Dpts(vector<Point3f> &pts3d) const
{
	assert(m_pts3d.size() == m_size.width * m_size.height);
	pts3d = m_pts3d;
}

/************************************************************************/
/*   CvxMarkerChessboard                                                */
/************************************************************************/
CvxMarkerChessboard::CvxMarkerChessboard()
{

}
CvxMarkerChessboard::~CvxMarkerChessboard()
{

}

bool CvxMarkerChessboard::SetConfigure(const CvSize &size, const float unitLength)
{
	assert(size.width > 5 && size.height>5);
	assert(unitLength > 0.0f);

	m_size = size;
	m_squareLen = unitLength;
	m_markerCenters.push_back(Point2f(-unitLength, -unitLength));
	m_markerCenters.push_back(Point2f(unitLength*size.width, -unitLength));
	m_markerCenters.push_back(Point2f(unitLength*size.width, unitLength*size.height));
	m_markerCenters.push_back(Point2f(-unitLength, unitLength*size.height));

	m_pts3d.resize(size.height*size.width);
	m_pts2d.resize(size.height*size.width);
	for (int y = 0; y<size.height; ++y)
	{
		for (int x = 0; x<size.width; ++x)
		{
			float f_x = x * unitLength;
			float f_y = y * unitLength;
			int idx = y * size.width + x;
			m_pts2d[idx] = Point2f(f_x, f_y);
			m_pts3d[idx] = Point3f(f_x, f_y, 0.0);
		}
	}
	return true;
}
bool CvxMarkerChessboard::SetMarkerCorners(float markerLength)
{
	assert(m_markerCenters.size() == 4);
	assert(markerLength > 0);

	m_markerCorners3d.resize(4 * 4);
	float h_size = markerLength/2;
	for (unsigned int i = 0; i<m_markerCenters.size(); ++i)
	{
		Point2f p = m_markerCenters[i];
		m_markerCorners3d[4*i]   = Point3f(p.x - h_size, p.y - h_size, 0);
		m_markerCorners3d[4*i+1] = Point3f(p.x + h_size, p.y - h_size, 0);
		m_markerCorners3d[4*i+2] = Point3f(p.x + h_size, p.y + h_size, 0);
		m_markerCorners3d[4*i+3] = Point3f(p.x - h_size, p.y + h_size, 0);
	}
	return true;
}

bool CvxMarkerChessboard::FindCorners(const IplImage *image, bool isTracking, 
									  const vector<CvxMarker>& preMarkers, 
									  vector<CvxMarker> &curMarker, vector<Point2f> &pts2d)const
{
	//check parameters
	assert(image);
	IplImage *grayImage = NULL;
	if (image->nChannels == 1)
	{
		grayImage = const_cast<IplImage*>(image);
	}
	else
	{
		grayImage = cvCreateImage(cvGetSize(image), IPL_DEPTH_8U, 1);
		assert(grayImage);
		cvCvtColor(image, grayImage, CV_BGR2GRAY);
	}
#define CvxMarkerChessboard_TEST 0
#if CvxMarkerChessboard_TEST 
	IplImage *showImage = cvCreateImage(cvGetSize(grayImage), IPL_DEPTH_8U, 3);
	cvCvtColor(grayImage, showImage, CV_GRAY2BGR);
	double tt = clock();	
#endif
	//get marker's position
	
	curMarker.clear();
	{
		MarkerCriteria criteria;		
		criteria._fastCheck = true;
		CvxMarkerChessboard::GetThreshold(cvSize(image->width, image->height), m_size, 
							 criteria._maxPixels, criteria._minPixels);
		int ret = CVX_MARKER_DIF_NUMBER;
		
		//tracking in four small image
		if (isTracking && preMarkers.size() == 4)
		{
			for (int i = 0; i<4; ++i)
			{
				CvRect r = preMarkers[i].m_subArea;
				IplImage *subImage = cvCreateImage(cvSize(r.width, r.height), IPL_DEPTH_8U, 1);
				cvSetImageROI(grayImage, r);
				cvCopy(grayImage, subImage);
				cvResetImageROI(grayImage);

				vector<CvxMarker> subMarker;

				int rst = CvxMarker::detect(subImage, subMarker, 1, criteria);			

				if (rst == CVX_MARKER_SUCCESS)
				{
					subMarker[0].AddOffset(r.x, r.y);
					curMarker.push_back(subMarker[0]);
				}
				else
				{
#if CvxMarkerChessboard_TEST
  					cvShowImage("subImage", subImage);
  					cvSaveImage("subImage.png", subImage);
  					cvWaitKey(0);
#endif
					cvReleaseImage(&subImage);
					curMarker.clear();
					break;
				}
				cvReleaseImage(&subImage);
			}
			if (curMarker.size() == 4)
			{
				ret = CVX_MARKER_SUCCESS;
			}
		}
		else
		{
			//detecting in whole image
			ret = CvxMarker::detect(grayImage, curMarker, 4, criteria);
		}		
		if (ret == CVX_MARKER_SUCCESS)
		{
			for (int i = 0; i<4; ++i)
			{
				curMarker[i].GetCenterPts();
				curMarker[i].GetSubArea(10, grayImage->width, grayImage->height);
			}
		}
		else
		{
		//	fprintf(stderr, "can not found markers!\n");
			if (grayImage != const_cast<IplImage*>(image))
			{
				cvReleaseImage(&grayImage);
			}
			return false;
		}
	}
#if CvxMarkerChessboard_TEST
	printf("marker tacking cost time = %f\n", clock() - tt);
	tt = clock();
#endif
	
	//get H
	assert(m_markerCenters.size() == 4);
	vector<Point2f> markerCenterVec;
	for (unsigned int i = 0; i<m_markerCenters.size(); ++i)
	{
		markerCenterVec.push_back(curMarker[i].m_centerPt);
	}
	Mat Homo = CvxSF::findHomography4Pts(m_markerCenters, markerCenterVec);
	
//	cvx::CvxMat_d d_Homo(&Homo);
#if CvxMarkerChessboard_TEST 
	cvCvtColor(grayImage, showImage, CV_GRAY2BGR);	
#endif
	//get chessboard corner position
	pts2d.resize(m_size.width * m_size.height);
	double *pData = (double*)(Homo.data);

	for (unsigned int i = 0; i<m_pts2d.size(); ++i)
	{
		Point2f p1 = m_pts2d[i];
		Point2f p2;
		float scale = 1.0;
		p2.x  = (float)(pData[0] * p1.x  + pData[1] * p1.y + pData[2]);
		p2.y  = (float)(pData[3] * p1.x  + pData[4] * p1.y + pData[5]);
		scale = (float)(pData[6] * p1.x  + pData[7] * p1.y + pData[8]);
		p2.x /= scale;
		p2.y /= scale;
		if(p2.x >=0 && p2.x < grayImage->width && p2.y >=0 && p2.y < grayImage->height)
		{
			pts2d[i] = p2;
		}
		else
		{
			if (grayImage != const_cast<IplImage*>(image))
			{
				cvReleaseImage(&grayImage);
			}
			pts2d.clear();
			return false;
		}		
	}

	vector<Point2f> rf = pts2d;
	int winLength = 11;
	if (grayImage->width < 800)
	{
		winLength = 5;
	}
	cv::cornerSubPix(Mat(grayImage), pts2d, Size(winLength, winLength), Size(-1, -1), 
						TermCriteria( CV_TERMCRIT_EPS+CV_TERMCRIT_ITER, 30, 0.1 ));
#if CvxMarkerChessboard_TEST
	printf("sub pix cost time = %f\n", clock()-tt);
#endif

	//check final points
	//@todo
	if (grayImage->width == 1920 && grayImage->height == 1080)
	{
		for (int i = 0; i<pts2d.size(); ++i)
		{
			if (fabs(rf[i].x - pts2d[i].x) > winLength + 1 || fabs(rf[i].y - pts2d[i].y) > winLength + 1) 
			{			
				pts2d.clear();
				if (grayImage != const_cast<IplImage*>(image))
				{
					cvReleaseImage(&grayImage);
				}
				return false;
			}
		}
	}

	if (grayImage != const_cast<IplImage*>(image))
	{
		cvReleaseImage(&grayImage);
	}
#if CvxMarkerChessboard_TEST 
	if (showImage)
	{
		cvReleaseImage(&showImage);
	}
#endif
	return true;
}

bool CvxMarkerChessboard::GetInsidePoints(const IplImage *image, vector<Point2f> &extremeCorners, 
										  vector<Point2f> &pts2d)const
{
	assert(image);

	//get a grayimage
	IplImage *grayImage = NULL;
	if (image->nChannels == 1)
	{
		grayImage = const_cast<IplImage *>(image);
	}
	else if (image->nChannels == 3)
	{
		grayImage = cvCreateImage(cvGetSize(image), IPL_DEPTH_8U, 1);
		assert(grayImage);
		cvCvtColor(image, grayImage, CV_BGR2GRAY);
	}

	bool ret = CvxSF::GetInsidePoints(grayImage, extremeCorners, m_size, pts2d, true);

	if (grayImage != const_cast<IplImage *>(image))
	{
		cvReleaseImage(&grayImage);
	}
	return ret;
}

void CvxMarkerChessboard::DrawCorners(const IplImage *image, const vector<Point2f> &pts)const
{
	assert(image);
	cv::drawChessboardCorners(Mat(image), m_size, pts, true);
}

float CvxMarkerChessboard::GetUnitLength(void)const
{
	return m_squareLen;
}
CvSize CvxMarkerChessboard::GetPatternSize(void)const
{
	return m_size;
}
void CvxMarkerChessboard::Get3Dpts(vector<Point3f> &pts3d) const
{
	assert(m_pts3d.size() == m_size.width * m_size.height);
	pts3d = m_pts3d;
}
void CvxMarkerChessboard::GetThreshold(const CvSize &imageSize, const CvSize &boardSize, int &thMax, int &thMin)
{
	float avg_w = 1.0f * imageSize.width/boardSize.width;
	float avg_h = 1.0f * imageSize.height/boardSize.height;
	float avg_max = std::max(avg_w, avg_h);
	thMax = (int)(avg_w * 4 * 1.2);
	thMin = thMax/6;
}

/************************************************************************/
/*   CvxMarkerChessboardExp
*/
/************************************************************************/
CvxMarkerChessboardExp::CvxMarkerChessboardExp()
{

}
CvxMarkerChessboardExp::~CvxMarkerChessboardExp()
{

}

bool CvxMarkerChessboardExp::SetConfigure(const CvSize &size, const float unitLength)
{
	assert(size.width > 5 && size.height>5);
	assert(unitLength > 0.0f);

	m_size = size;
	m_squareLen = unitLength;
	m_markerCenters.push_back(Point2f(0, 0));
	m_markerCenters.push_back(Point2f(unitLength*(size.width-1), 0));
	m_markerCenters.push_back(Point2f(unitLength*(size.width-1), unitLength*(size.height-1)));
	m_markerCenters.push_back(Point2f(0, unitLength*(size.height-1)));

	m_pts3d.resize(size.height*size.width);
	m_pts2d.resize(size.height*size.width);
	for (int y = 0; y<size.height; ++y)
	{
		for (int x = 0; x<size.width; ++x)
		{
			float f_x = x * unitLength;
			float f_y = y * unitLength;
			int idx = y * size.width + x;
			m_pts2d[idx] = Point2f(f_x, f_y);
			m_pts3d[idx] = Point3f(f_x, f_y, 0.0);
		}
	}

	//change marker center position to nearby corners
	m_pts2d[0] = m_pts2d[1];
	m_pts2d[size.width-1] = m_pts2d[size.width-2];
	m_pts2d[size.width * (size.height - 1) + size.width - 1] = m_pts2d[size.width * (size.height - 1) + size.width - 2];
	m_pts2d[size.width * (size.height - 1)] = m_pts2d[size.width * (size.height - 1) + 1];	

	return true;
}
bool CvxMarkerChessboardExp::SetMarkerCorners(float markerLength)
{
	assert(m_markerCenters.size() == 4);
	assert(markerLength > 0);

	m_markerCorners3d.resize(4 * 4);
	float h_size = markerLength/2;
	for (unsigned int i = 0; i<m_markerCenters.size(); ++i)
	{
		Point2f p = m_markerCenters[i];
		m_markerCorners3d[4*i]   = Point3f(p.x - h_size, p.y - h_size, 0);
		m_markerCorners3d[4*i+1] = Point3f(p.x + h_size, p.y - h_size, 0);
		m_markerCorners3d[4*i+2] = Point3f(p.x + h_size, p.y + h_size, 0);
		m_markerCorners3d[4*i+3] = Point3f(p.x - h_size, p.y + h_size, 0);
	}
	return true;
}


bool CvxMarkerChessboardExp::FindCorners(const IplImage *image, bool isTracking, 
									  const vector<CvxMarker>& preMarkers, 
									  vector<CvxMarker> &curMarker, vector<Point2f> &pts2d)const
{
	//check parameters
	assert(image);
	IplImage *grayImage = NULL;
	if (image->nChannels == 1)
	{
		grayImage = const_cast<IplImage*>(image);
	}
	else
	{
		grayImage = cvCreateImage(cvGetSize(image), IPL_DEPTH_8U, 1);
		assert(grayImage);
		cvCvtColor(image, grayImage, CV_BGR2GRAY);
	}
#define CvxMarkerChessboardExp_TEST 0
#if CvxMarkerChessboardExp_TEST 
	IplImage *showImage = cvCreateImage(cvGetSize(grayImage), IPL_DEPTH_8U, 3);
	cvCvtColor(grayImage, showImage, CV_GRAY2BGR);
	double tt = clock();	
#endif
	//get marker's position

	curMarker.clear();
	{
		MarkerCriteria criteria;
		criteria._minPixels = 100;
		criteria._maxPixels = 550;
		criteria._fastCheck = true;
		int ret = CVX_MARKER_DIF_NUMBER;

		//tracking in four small image
		if (isTracking && preMarkers.size() == 4)
		{
			for (int i = 0; i<4; ++i)
			{
				CvRect r = preMarkers[i].m_subArea;
				IplImage *subImage = cvCreateImage(cvSize(r.width, r.height), IPL_DEPTH_8U, 1);
				cvSetImageROI(grayImage, r);
				cvCopy(grayImage, subImage);
				cvResetImageROI(grayImage);

				vector<CvxMarker> subMarker;

				int rst = CvxMarker::detect(subImage, subMarker, 1, criteria);			

				if (rst == CVX_MARKER_SUCCESS)
				{
					subMarker[0].AddOffset(r.x, r.y);
					curMarker.push_back(subMarker[0]);
				}
				else
				{
					//  					cvShowImage("subImage", subImage);
					//  					cvSaveImage("subImage.png", subImage);
					//  					cvWaitKey(0);
					cvReleaseImage(&subImage);
					curMarker.clear();
					break;
				}
				cvReleaseImage(&subImage);
			}
			if (curMarker.size() == 4)
			{
				ret = CVX_MARKER_SUCCESS;
			}
		}
		else
		{
			//detecting in whole image
			ret = CvxMarker::detect(grayImage, curMarker, 4, criteria);
		}		
		if (ret == CVX_MARKER_SUCCESS)
		{
			for (int i = 0; i<4; ++i)
			{
				curMarker[i].GetCenterPts();
				curMarker[i].GetSubArea(10, grayImage->width, grayImage->height);
			}
		}
		else
		{
			//	fprintf(stderr, "can not found markers!\n");
			if (grayImage != const_cast<IplImage*>(image))
			{
				cvReleaseImage(&grayImage);
			}
			return false;
		}
	}
#if CvxMarkerChessboardExp_TEST
	printf("marker tacking cost time = %f\n", clock() - tt);
	tt = clock();
#endif

	//get H
	assert(m_markerCenters.size() == 4);
	vector<Point2f> markerCenterVec;
	for (unsigned int i = 0; i<m_markerCenters.size(); ++i)
	{
		markerCenterVec.push_back(curMarker[i].m_centerPt);
	}
	Mat Homo = CvxSF::findHomography4Pts(m_markerCenters, markerCenterVec);

#if CvxMarkerChessboardExp_TEST
	//draw marker corner position
	cvCvtColor(grayImage, showImage, CV_GRAY2BGR);
	for (unsigned int i = 0; i<m_markerCenters.size(); ++i)
	{
		for (int j = 0; j<4; ++j)
		{
			cv::circle(Mat(showImage), curMarker[i].m_corners[j], 1, cvScalar(0,0,255,255));			
		}	
	}
	cvSaveImage("initial_marker_corners.png", showImage);
#endif

	//get chessboard corner position
	pts2d.resize(m_size.width * m_size.height);
	double *pData = (double*)(Homo.data);

	for (unsigned int i = 0; i<m_pts2d.size(); ++i)
	{
		Point2f p1 = m_pts2d[i];
		Point2f p2;
		float scale = 1.0;
		p2.x  = (float)(pData[0] * p1.x  + pData[1] * p1.y + pData[2]);
		p2.y  = (float)(pData[3] * p1.x  + pData[4] * p1.y + pData[5]);
		scale = (float)(pData[6] * p1.x  + pData[7] * p1.y + pData[8]);
		p2.x /= scale;
		p2.y /= scale;
		assert(p2.x >=0 && p2.x < grayImage->width);
		assert(p2.y >=0 && p2.y < grayImage->height);
		pts2d[i] = p2;
	}

#if CvxMarkerChessboardExp_TEST 
	cvCvtColor(grayImage, showImage, CV_GRAY2BGR);
	cv::drawChessboardCorners(Mat(showImage), cvSize(16, 12), pts2d, true);

//	cvShowImage("initial position", showImage);
//	cvSaveImage("initial_pos.png", showImage);
//	cvWaitKey(0);
#endif

	vector<Point2f> rf = pts2d;
	int winLength = 11;
	if (grayImage->width < 800)
	{
		winLength = 5;
	}
	cv::cornerSubPix(Mat(grayImage), pts2d, Size(winLength, winLength), Size(-1, -1), 
					 TermCriteria( CV_TERMCRIT_EPS+CV_TERMCRIT_ITER, 30, 0.1 ));

#if CvxMarkerChessboardExp_TEST
	printf("sub pix cost time = %f\n", clock()-tt);
#endif
	//check final points
	for (int i = 0; i<pts2d.size(); ++i)
	{
		if (fabs(rf[i].x - pts2d[i].x) > 15 || fabs(rf[i].y - pts2d[i].y) > 15) 
		{
			pts2d.clear();
			if (grayImage != const_cast<IplImage*>(image))
			{
				cvReleaseImage(&grayImage);
			}
			return false;
		}
	}

	pts2d[0] = curMarker[0].m_centerPt;
	pts2d[m_size.width-1] = curMarker[1].m_centerPt;
	pts2d[m_size.width * (m_size.height - 1) + m_size.width - 1] = curMarker[2].m_centerPt;
	pts2d[m_size.width * (m_size.height - 1)] = curMarker[3].m_centerPt;


	if (grayImage != const_cast<IplImage*>(image))
	{
		cvReleaseImage(&grayImage);
	}
#if CvxMarkerChessboardExp_TEST 
	if (showImage)
	{
		cvReleaseImage(&showImage);
	}
#endif
	return true;
}

bool CvxMarkerChessboardExp::GetInsidePoints(const IplImage *image, vector<Point2f> &extremeCorners, 
										    vector<Point2f> &pts2d)const
{
	return false;
}

void CvxMarkerChessboardExp::DrawCorners(const IplImage *image, const vector<Point2f> &pts)const
{
	assert(image);
	cv::drawChessboardCorners(Mat(image), m_size, pts, true);
}

float CvxMarkerChessboardExp::GetUnitLength(void)const
{
	return m_squareLen;
}
CvSize CvxMarkerChessboardExp::GetPatternSize(void)const
{
	return m_size;
}
void CvxMarkerChessboardExp::Get3Dpts(vector<Point3f> &pts3d) const
{
	assert(m_pts3d.size() == m_size.width * m_size.height);
	pts3d = m_pts3d;
}

